#include "../velocitymode/velocitymode.h"
#include "velocityplan.h"

#ifndef max
#define max(a,b)	(a>b?a:b)
#endif

int Linear_Plan(LINEAR_DATA *plineardata)
{
	
	float vs,ve,vm,acc,s;
	vs = plineardata->vs;
	ve = plineardata->ve;
	vm = plineardata->vm;
	acc = plineardata->acc;
	s = plineardata->s;
	
	//cal acc part
	plineardata->t1 = Linear_AccTime(vs, vm, acc);
	plineardata->p1 = Linear_AccDis(vs, vm, acc);
	
	
	//cal steady part
	plineardata->p2 = s - Linear_DecDis(vm, ve, acc);
	plineardata->t2 = plineardata->t1 + (plineardata->p2 - plineardata->p1)/vm;
	
	//
	plineardata->t = plineardata->t2 + Linear_DecTime(vm, ve, acc);	
	return VEL_OK;
	
}

int Linear_Cal(LINEAR_DATA *plineardata, float t, float *pos, float *vc)
{
	if(t < plineardata->t1)
	{
		*vc = plineardata->vs + t * plineardata->acc;
		*pos = plineardata->vs * t + plineardata->acc * t * t * 0.5f;
	}
	else if(t < plineardata->t2)
	{
		*vc = plineardata->vm;
		*pos = plineardata->p1 + (t - plineardata->t1) * plineardata->vm;
	}
	else if(t < plineardata->t)
	{
		*vc = plineardata->vm - (t - plineardata->t2)*plineardata->acc;
		*pos = plineardata->p2 + plineardata->acc * (t - plineardata->t2) * (t - plineardata->t2) * 0.5f;
	}
	else
	{
		*pos = plineardata->s;
		*vc = plineardata->ve;
		return VEL_ENDPART;
	}
	return VEL_OK;
}



